from utils.interval import Interval
from utils import MAXFLOAT, floatcompare as compare

from cranesched.routing.geometry import Line
from cranesched.routing.path import Path
from cranesched.routing.bounds import BoundsManager


class Router(BoundsManager):
    """This object can be used to plan valid routes (described by line segments) for satisfying 
    movement requests while avoiding collisions between Movable1D objects along a specific 
    movement axis."""
    def earliest_fit(self, movable, target, duration=0.0, window=None, verbose=False):
        """Finds the earliest possible fit (considering the object's lower and upper bounds) of 
        the given request to move 'movable' into position 'target', and stay in that position for 
        'duration' seconds within interval 'window'. Returns the start time of the interval which 
        satisfies the request, i.e. the time when the object reaches position 'target'."""
        log = do_print if verbose else no_print
        if window is None:
            window = Interval(-MAXFLOAT, +MAXFLOAT)
        delta = target - movable.position
        earliest_arrival = movable.time + abs(delta) / float(movable.speed)
        earliest_start = max(earliest_arrival, window.start)
        if compare.gt(earliest_start + duration, window.end):
            # even if we moved immediately into the target position, we wouldn't be 
            # able to complete the action before the end of the satisfiability window
            return None
        
        entry = self.entry[movable]
        oob = entry.out_of_bounds(earliest_start, target)
        t0 = t1 = earliest_start
        log("out of bounds=%s, interval=[%s, %s]", oob, t0, t1)
        if compare.eq(earliest_start, window.end):
            # surely 'duration' is zero, otherwise the condition  
            # on the previous if statement would be true
            return None if oob else earliest_start
            
        eligible = Line.Segment(earliest_start, target, window.end, target)
        for (t2, _), _, _ in Path.multipath_intersections((entry.lb, entry.ub), eligible):
            log("intersection at t=%s", t2)
            if compare.eq(t1, t2):
                log("skipped")
                continue
            oob, t0, t1 = update_out_of_bounds(oob, t0, t1, t2, target, entry)
            log("out of bounds=%s, interval=[%s, %s]", oob, t0, t1)
            if not oob and compare.geq(t1 - t0, duration):
                return t0
        if compare.lt(t1, window.end):
            oob, t0, t1 = update_out_of_bounds(oob, t0, t1, window.end, target, entry)
        log("out of bounds=%s, interval=[%s, +INF)", oob, t0)
        if not oob and compare.leq(t0 + duration, window.end):
            return t0
        return None
        
    def compute_trajectory(self, movable, target, time):
        """Computes a trajectory for a movable to reach a target position by the specified time. 
        The given target must obviously lie between the lower and upper bounds for the movable's 
        position. Returns a sequence of points that should be appended to the Movable's path."""
        points = [(time, target)]
        entry = self.entry[movable]
        path = movable.path
        # Loop until we join with the movable's current time
        assert compare.geq(time, path.time)
        while compare.gt(time, path.time):
            moved = False
            # 1) See if we're at one of the bounds, and follow it if necessary.
            if compare.leq(time, entry.lb.time):
                lb_segment = entry.lb.segments_at(time)[0]
                if compare.eq(lb_segment.y_at(time), target) and compare.leq(lb_segment.m, 0.0):
                    time, target = lb_segment.p0
                    moved = True
            if not moved and compare.leq(time, entry.ub.time):
                ub_segment = entry.ub.segments_at(time)[0]
                if compare.eq(ub_segment.y_at(time), target) and compare.geq(ub_segment.m, 0.0):
                    time, target = ub_segment.p0
                    moved = True
            # 2) Otherwise, we're between the two bounds, so we need 
            # to approach one of them with a horizontal segment.
            if not moved:
                horizontal = Line.Segment(path.time, target, time, target)
                iterator = Path.multipath_intersections(entry.bounds, horizontal, reverse=True)
                for (t, p), _, _ in iterator:
                    if compare.lt(t, time):
                        time, target = t, p
                        moved = True
                        break
            assert moved
            points.append((time, target))
        assert compare.eq(path.time, time)
        assert compare.eq(path.position, target)
        points.pop()
        points.reverse()
        return points
        
        
def update_out_of_bounds(oob, t0, t1, t2, pos, entry):
    """Auxiliary function used by Router.earliest_fit()."""
    if oob != entry.out_of_bounds((t1 + t2) * 0.5, pos):
        return not oob, t1, t2
    return oob, t0, t2
    
    
def do_print(message, *args):
    print message % args
    
def no_print(message, *args):
    pass
    
    
def _test(seed=None, movables=3, periods=(10, 20), speeds=(1,2,3), sizes=(1,2)):
    from cranesched.routing.movable1d import Movable1D
    import random
    import time
    
    # generate random paths for the movables
    if seed is None:
        seed = int(time.time() * 1000)
        print "no seed provided, using current time in ms...", seed 
    random.seed(seed)
    router = Router()
    for i in xrange(movables):
        movable = Movable1D("x" + str(i), time=0.0, position=0.0, 
                            speed=random.choice(speeds), 
                            size=random.choice(sizes))
        for j in xrange(random.randint(*periods)):
            direction = random.randint(-1, +1)
            movable.path.append(j+1, movable.path.position + movable.speed * direction)
        router.add(movable)
    # offset the position of the movables so that there are no collisions
    def remove_collisions(objs, i):
        obj_i = objs[i]
        size = obj_i.size * 0.5
        for j in xrange(i+1, len(objs)):
            obj_j = objs[j]
            size += obj_j.size * 0.5
            temp = obj_j.path.deepcopy()
            temp.offset_position(-size)
            offset = 0
            if temp.positions[0] < obj_i.path.positions[0]:
                offset = obj_i.path.positions[0] - temp.positions[0]
                temp.offset_position(offset)
            while obj_i.path.collides(temp):
                offset += 1
                temp.offset_position(+1)
            if offset > 0:
                for k in xrange(j, len(objs)):
                    objs[k].path.offset_position(offset)
            size += obj_j.size * 0.5
            
    objs = router.sequence
    for i in xrange(movables-1):
        remove_collisions(objs, i)
        
    # setup router "walls"
    positions = set()
    for movable in router.sequence:
        positions.update(movable.path.positions)
    router.set_limits(min(positions) - router.sequence[0].size, 
                      max(positions) + router.sequence[-1].size)
    router.update()
    router.show()
    return router
    
    
